import cv2
import numpy as np
import math
import matplotlib.pyplot as plt

class Point2d:
    def __init__(self, x, y):
        self.x = x
        self.y = y
    
    def distance_to(self, other_point):
        # return math.sqrt((self.x - other_point.x) ** 2 + (self.y - other_point.y)**2)
        return abs(self.x - other_point.x) + abs(self.y - other_point.y)

    def dot_to(self, other_point):
        return self.x * other_point.x + self.y*other_point.y

class Point2dSet:
    def __init__(self, first_point2d):
        super().__init__()
        self.points = [first_point2d]
        self.center = first_point2d
    
    def append(self, point2d):
        self.points.append(point2d)

        sum_x = 0
        sum_y = 0
        for p in self.points:
            sum_x += p.x
            sum_y += p.y
        
        self.center = Point2d(sum_x / len(self.points),sum_y / len(self.points) )
def calc_line_cross_point(line1, line2):
    if line1.horizontal:
        if line2.horizontal:
            return None
        elif line2.vertical:
            return Point2d(line2.p1.x, line1.p1.y)
        else:
            y = line1.p1.y
            x = line2.calc_x_value(y)
            return Point2d(x, y)
    elif line1.vertical:
        if line2.horizontal:
            return Point2d(line1.p1.x, line2.p1.y)
        elif line2.vertical:
            return None
        else:
            x = line1.p1.x
            y = line2.calc_y_value(x)
            return Point2d(x, y)
    else:
        if line2.horizontal:
            y = line2.p1.y
            x = line1.calc_x_value(y)
            return Point2d(x, y)
        elif line2.vertical:
            x = line2.p1.x
            y = line1.calc_y_value(x)
            return Point2d(x, y)
        else:
           x = (line2.b - line1.b) / (line1.k - line2.k)
           y = line1.calc_y_value(x)
           return Point2d(x, y)
class Line:
    def __init__(self, point1, point2):
        super().__init__()
        self.p1 = point1
        self.p2 = point2
        self.horizontal = False # 水平线
        self.vertical = False   # 垂直线
        self.k = 0
        self.b = 0
        self.len = math.sqrt((point2.x - point1.x)**2 + (point2.y - point1.y)**2) 
        self.direct_vector = Point2d((point2.x - point1.x)/self.len, (point2.y - point1.y)/self.len)

        if point1.x == point2.x:
            self.vertical = True
        elif point1.y == point2.y:
            self.horizontal = True
        else:
            deta_x = point1.x - point2.x
            deta_y = point1.y - point2.y
            self.k = deta_y / deta_x
            self.b = point1.y - self.k * point1.x

    def calc_y_value(self, x):
        if self.horizontal:
            return self.p1.y
        elif self.vertical:
            raise Exception('chui zhi')
        else:
            return self.k * x + self.b

    def calc_x_value(self, y):
        if self.horizontal:
            raise Exception('ping xing')
        elif self.vertical:
            return self.p1.x
        else:
            return (y - b) / self.k

    def angle(self, other_line):
        return math.acos(self.direct_vector.dot_to(other_line.direct_vector))

def find_point_sets(lines, distance=200):
    result = [Point2dSet(Point2d(lines[0, 0,0], lines[0, 0,1]))]
    for line in lines:
        p = Point2d(line[0,0], line[0,1])
        for pointset in result:
            if p.distance_to(pointset.center) <= distance:
                pointset.append(p)
            else:
                result.append(Point2dSet(p))

    return result

def find_cross():
    # 读取图像
    src = cv2.imread('a.jpg')
    if src is None:
        print("load img failed!\n")
        return
    else:
        print(type(src), src.shape)

    # 转灰度
    gray_img = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
    gray_img = cv2.GaussianBlur(gray_img, (5,5), 0.1, borderType=cv2.BORDER_DEFAULT)
    # 二值化
    (th, binary_img) = cv2.threshold(gray_img, 100, 255, cv2.THRESH_BINARY|cv2.THRESH_OTSU)
    
    # 开运算,消除小白点
    kernel = np.ones((5,5), np.uint8)
    opening_img = cv2.morphologyEx(binary_img, cv2.MORPH_OPEN, kernel)
    # 闭运算
    opening_img = cv2.morphologyEx(opening_img, cv2.MORPH_CLOSE, kernel)
    
    # Canny
    canny_img = cv2.Canny(opening_img, 20, 100)
    # _,contours,hierarchy = cv2.findContours(opening_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    # img_with_contours = cv2.drawContours(src,contours, -1,(0,255,0),4)
    # print("contours count = ", len(contours))
    # for contour in contours:
    #     moment = cv2.moments(contour)
    #     point = np.zeros(2, np.int32)
    #     point[0] = moment['m10'] / moment['m00']
    #     point[1] = moment['m01'] / moment['m00']
    #     cv2.circle(img_with_contours, (point[0], point[1]), 3, (0,0,255), 8)
    # lines = cv2.HoughLinesP(canny_img, 1, np.pi / 180, 10)
    # for line in lines:
    #     x1 = line[0, 0]
    #     y1 = line[0, 1]
    #     x2 = line[0, 2]
    #     y2 = line[0, 3]

        # cv2.line(gray_img, (x1,y1), (x2, y2), (0, 255, 0)) 
    lines = cv2.HoughLines(canny_img, 1, np.pi/180, 100)
    # x_p = np.zeros(len(lines))
    # y_p = np.zeros(len(lines))
    # for i in range(len(lines)):
    #     x_p[i] = lines[i, 0, 0]
    #     y_p[i] = lines[i, 0, 1]
    # plt.scatter(x_p, y_p, s=75, c='r', alpha=0.5)
    # plt.show()
    
    all_lines = []
    for line in lines:
        rho = line[0,0]
        theta = line[0,1]
        a = np.cos(theta)   #theta是弧度
        b = np.sin(theta)
        x0 = a * rho    #代表x = r * cos（theta）
        y0 = b * rho    #代表y = r * sin（theta）
        x1 = int(x0 + 1000 * (-b)) #计算直线起点横坐标
        y1 = int(y0 + 1000 * a)    #计算起始起点纵坐标
        x2 = int(x0 - 1000 * (-b)) #计算直线终点横坐标
        y2 = int(y0 - 1000 * a)    #计算直线终点纵坐标    注：这里的数值1000给出了画出的线段长度范围大小，数值越小，画出的线段越短，数值越大，画出的线段越长
        cv2.line(gray_img, (x1, y1), (x2, y2), (0, 0, 255), 2)    #点的坐标必须是元组，不能是列表。
        all_lines.append(Line(Point2d(x1, y1), Point2d(x2, y2)))
    
    cross_points = []
    sum_x = 0
    sum_y = 0
    for line in all_lines:
        for l2 in all_lines:
            a = line.angle(l2)
            if abs( a- np.pi / 2) <= 0.1:
                p = calc_line_cross_point(line, l2)
                if p:
                    cross_points.append(p)
                    sum_x += p.x
                    sum_y += p.y
            else:
                print("pass")
                    #cv2.circle(gray_img, (int(p.x + 0.5), int(p.y + 0.5)), 3, (0,0,255), 3)
    cv2.circle(src, (int(sum_x/len(cross_points) + 0.5), int(sum_y/len(cross_points) + 0.5)), 3, (255,0,0), 3)
    # goodfeatures_corners = cv2.goodFeaturesToTrack(opening_img, 8, 0.01, 10)
    # for p in goodfeatures_corners:
    #     x = p[0,0]
    #     y = p[0,1]
    #     cv2.circle(gray_img, (x, y), 3, (0,0,255), 3)
    
    # 显示
    cv2.imshow("gray_src_{}".format(src.shape), src)
    cv2.imshow("binary_src_th%d" % th, binary_img)
    cv2.imshow("opening_img_5x5", opening_img)
    cv2.waitKey(0)
    
if __name__ == "__main__":
    find_cross()
